#include "stdafx.h"
#include "Vector3d.h"
#include "Matrix3d.h"
#include "GeomUtils.h"

using namespace stl;

CVector3D::CVector3D()
{
	x = y = z = 0.0;
}

CVector3D::CVector3D(double dx, double dy, double dz)
{
	x = dx;
	y = dy;
	z = dz;
}

CVector3D::CVector3D(const double * pVec)
{
	x = y = z = 0.0;
	if (pVec != nullptr)
	{
		x = pVec[0];
		y = pVec[1];
		z = pVec[2];
	}
}

CVector3D::CVector3D(const VECTOR3D& v)
{
	x = v.x;
	y = v.y;
	z = v.z;
}

CVector3D::~CVector3D()
{
}

CVector3D CVector3D::operator+(VECTOR3D v) const
{
	return CVector3D(x + v.x, y + v.y, z + v.z);
}

void CVector3D::operator+=(VECTOR3D v)
{
	x += v.x;
	y += v.y;
	z += v.z;
}

CVector3D CVector3D::operator-(VECTOR3D v) const
{
	return CVector3D(x - v.x, y - v.y, z - v.z);
}

void CVector3D::operator-=(VECTOR3D v)
{
	x -= v.x;
	y -= v.y;
	z -= v.z;
}

CVector3D CVector3D::operator*(double d) const
{
	return CVector3D(x * d, y * d, z * d);
}

void CVector3D::operator*=(double d)
{
	x *= d;
	y *= d;
	z *= d;
}

CVector3D CVector3D::operator/(double d) const
{
	return CVector3D(x / d, y / d, z / d);
}

void CVector3D::operator/=(double d)
{
	x /= d;
	y /= d;
	z /= d;
}

CVector3D CVector3D::operator*(VECTOR3D v) const
{
	return CVector3D(y*v.z - v.y*z, v.x*z - x * v.z, x*v.y - v.x*y);
}

CVector3D CVector3D::CrossProduct(const VECTOR3D& v)
{
	return CVector3D(y*v.z - v.y*z, v.x*z - x * v.z, x*v.y - v.x*y);
}

double CVector3D::operator|(VECTOR3D v) const
{
	return x * v.x + y * v.y + z * v.z;
}

double CVector3D::DotProduct(const VECTOR3D& v)
{
	return x * v.x + y * v.y + z * v.z;
}

CVector3D CVector3D::operator*(const MATRIX3d & matrix) const
{
	double rx, ry, rz, sc;
	rx = x * matrix.A[0][0] + y * matrix.A[1][0] + z * matrix.A[2][0] + matrix.A[3][0];
	ry = x * matrix.A[0][1] + y * matrix.A[1][1] + z * matrix.A[2][1] + matrix.A[3][1];
	rz = x * matrix.A[0][2] + y * matrix.A[1][2] + z * matrix.A[2][2] + matrix.A[3][2];
	sc = x * matrix.A[0][3] + y * matrix.A[1][3] + z * matrix.A[2][3] + matrix.A[3][3];
	rx /= sc;
	ry /= sc;
	rz /= sc;
	return CVector3D(rx, ry, rz);
}

void CVector3D::operator*=(const MATRIX3d & matrix)
{
	(*this) = (*this)*matrix;
}

double CVector3D::GetLength() const
{
	return sqrt(x * x + y * y + z * z);
}

double CVector3D::GetLengthXY() const
{
	return sqrt(x * x + y * y);
}

double CVector3D::GetLengthYZ() const
{
	return sqrt(y * y + z * z);
}

double CVector3D::GetLengthZX() const
{
	return sqrt(x * x + z * z);
}

CVector3D CVector3D::Normal() const
{
	double dLeng = GetLength();
	return CVector3D(x / dLeng, y / dLeng, z / dLeng);
}

void CVector3D::Normallize()
{
	double dleng = GetLength();
	x /= dleng;
	y /= dleng;
	z /= dleng;
}

bool CVector3D::IsZeroLength() const
{
	return GeEqual(GetLength(), 0.0);
}

void CVector3D::set(double dx, double dy, double dz)
{
	x = dx;
	y = dy;
	z = dz;
}

double stl::CVector3D::AngleTo(const CVector3D& vec)
{
	double dot = (*this) | vec;
	double dLength = GetLength() * vec.GetLength();
	double dAngle = acos(dot / dLength);
	return dAngle;
}

CVector3D& CVector3D::Negate()
{
	x = -x;
	y = -y;
	z = -z;
	return *this;
}